#include "Animation.h"
#include "../Utility/Math/MathHelper.h"

#include <fstream>
using std::ifstream;
using std::ios_base;

#ifdef _DEBUG
#include "../Utility/Misc/DebugOutput.h"
#endif

void Animation::LoadAnimation(string fileName)
{
	ifstream in;
	in.open(fileName, ios_base::binary);

	char auxName[256];

	size_t length;
	double auxDuration;
	size_t iNumKeys;
	size_t numBones;

	in.read((char*) &length, sizeof(size_t));
	in.read((char*) &auxName, length);
	in.read((char*) &auxDuration, sizeof(auxDuration));
	duration = (float)auxDuration;

	name = auxName;
	name = name.substr(0,name.length()-5);

	in.read((char*) &numBones, sizeof(numBones));

	for(size_t i = 0; i<numBones; ++i)
	{
		AnimatedBone animBone;
		in.read((char*) &animBone.BoneIndex, sizeof(animBone.BoneIndex));
		in.read((char*) &iNumKeys, sizeof(iNumKeys));

		size_t size = sizeof(animBone.keyFrames[0]);
		animBone.keyFrames.resize(iNumKeys);
		in.read((char*)&animBone.keyFrames[0], sizeof(animBone.keyFrames[0])*iNumKeys);
		animatedBones.push_back(animBone);
	}

	boneFrames.resize(animatedBones.size());
}

void Animation::PlayAnimation(float _dt)
{
	unsigned int prevFrame = 0;
	unsigned int nextFrame = 0;
	float tweenTime;
	currentTime+=_dt;

	while(currentTime > duration)
	{
		currentTime -= duration;
	}
	while(currentTime < 0)
	{
		currentTime += duration;
	}

	for(unsigned int keyIter = 0; keyIter <animatedBones[0].keyFrames.size(); ++keyIter)
	{
		if(keyIter < animatedBones[0].keyFrames.size() - 1)
		{
			tweenTime = animatedBones[0].keyFrames[keyIter+1].keyTime - animatedBones[0].keyFrames[keyIter].keyTime;
		}
		else
		{
			tweenTime = duration - animatedBones[0].keyFrames[keyIter].keyTime;
		}

		if(currentTime < animatedBones[0].keyFrames[keyIter].keyTime + tweenTime)
		{
			prevFrame = keyIter;
			nextFrame = keyIter + 1;

			if(nextFrame >= animatedBones[0].keyFrames.size())
			{
				nextFrame = 0;
			}
			break;
		}
	}

	float timeBetweenFrame = currentTime - animatedBones[0].keyFrames[prevFrame].keyTime;
	float alpha = timeBetweenFrame / tweenTime;

	for(unsigned int boneIter = 0; boneIter < animatedBones.size(); ++boneIter)
	{
		Bone prevBoneFrame;
		prevBoneFrame.SetBoneIndex(animatedBones[boneIter].BoneIndex);
		prevBoneFrame.GetLocalMat() = animatedBones[boneIter].keyFrames[prevFrame].localMat;
		prevBoneFrame.GetWorldMat() = animatedBones[boneIter].keyFrames[prevFrame].worldMat;

		Bone nextBoneFrame;
		nextBoneFrame.SetBoneIndex(animatedBones[boneIter].BoneIndex);
		nextBoneFrame.GetLocalMat() = animatedBones[boneIter].keyFrames[nextFrame].localMat;
		nextBoneFrame.GetWorldMat() = animatedBones[boneIter].keyFrames[nextFrame].worldMat;

		Bone newBoneFrame = prevBoneFrame;

		MathHelper::InterpolateMatrices(prevBoneFrame.GetWorldMat(), nextBoneFrame.GetWorldMat(),alpha,newBoneFrame.GetWorldMat());

		boneFrames[boneIter] = newBoneFrame;
	}
}

void Animation::ApplySmoothSkinning(AnimatedMeshDataPNT*& mesh)
{
	skinVerts.vertices.clear();
	skinVerts.vertices.resize(mesh->vertices.size());
	memcpy(&skinVerts.vertices[0],&mesh->vertices[0], sizeof(mesh->vertices)*mesh->vertices.size());

	for( unsigned int nCurVert = 0; nCurVert < mesh->vertices.size(); nCurVert++ )
	{
		for( unsigned int nCurJoint = 0; nCurJoint < mesh->weights[nCurVert].size(); nCurJoint++ )
		{
			XMFLOAT3 vertices = XMFLOAT3(	mesh->vertices[nCurVert].position.x,
											mesh->vertices[nCurVert].position.y,
											mesh->vertices[nCurVert].position.z);

			XMVECTOR curVert = XMLoadFloat3(&vertices);

			JointInfluence joint = mesh->weights[nCurVert][nCurJoint];
			XMFLOAT4X4 bindPose = mesh->joints[joint.boneIndex].transformFrame.GetWorldMat();
			XMFLOAT4X4 currentPose;
			currentPose = bindPose;

			if(!boneFrames.empty())
			{
				currentPose = boneFrames[joint.boneIndex].GetWorldMat();
			}
			XMFLOAT4X4 bindPoseInv;
			MathHelper::OrthoNormalInverse(bindPoseInv, bindPose);
			XMVECTOR BxV;
			XMVECTOR CxBxV;
			MathHelper::VectorByMatrixMult(curVert, bindPoseInv, BxV);
			MathHelper::VectorByMatrixMult(BxV, currentPose, CxBxV); 
			CxBxV *= joint.weight;
			XMFLOAT3 CxBxVf3;
			XMStoreFloat3(&CxBxVf3, CxBxV);
			skinVerts.vertices[nCurVert].position.x += CxBxVf3.x;
			skinVerts.vertices[nCurVert].position.y += CxBxVf3.y;
			skinVerts.vertices[nCurVert].position.z += CxBxVf3.z;
		}
	}
}